/**
 * @file main.cpp
 * @brief Code for XXX driver
 * @author He Shaohua (hsh15529870639@foxmail.com)
 * @date 2024-03-20
 * @version 1.0
 * @copyright Copyright (c) 2024, SNUT-Chaoyue Team
 * ****************************************************************************************************
 * @par Change Log:
 * <table>
 * <tr><th>Date         <th>Version       <th>Author      <th>Description
 * <tr><td>2024-03-20   <td> 1.0          <td>He Shaohua  <td>Creator
 *
 * </table>
 * ==================================----How to use this driver----=====================================
 * [..]
 * @note
 *   -#
 *
 * @warning
 *   -#
 *
 * *****************************************************************************************************
 * @attention
 *
 * This document is required and please understand any bugs that may occur in the document.
 * If you want to modify this file, after the code update is completed, update the version number in time,
 * indicate the update date, and fill in the changes
 * Make sure that the submitted code does not have many bugs.
 *
 * All rights reserved.
 * *****************************************************************************************************
 */
#include "rclcpp/rclcpp.hpp"
#include <functional>
#include <memory>
#include <chrono>
#include <string>
#include "serial_device.h"

using namespace std;
using namespace rclcpp;

size_t i = 0;
uint8_t data_size = 0;
uint8_t buffer[6] = {1, 2, 3, 4, 5, 6};
void RX_CallBack(uint8_t command, uint8_t *buffer, size_t size)
{
  i++;
  cout << "Size is " << size << ",Twice is " << i
       << endl;
}
ConnectDriver::Class_ConnectDriver ser(RX_CallBack);

class Serial : public Node
{
private:
  rclcpp::TimerBase::SharedPtr timer_receive;
  rclcpp::TimerBase::SharedPtr timer_send;
  void receive_callback()
  {
    ser.Receive();
  }
  void send_callback()
  {
    ser.Send(buffer, sizeof(buffer));
    RCLCPP_INFO(this->get_logger(), "Send_sucess!");
  }

public:
  Serial(string name) : Node(name)
  {
    RCLCPP_INFO(this->get_logger(), "Serial Connect Success!\n");
    // 接收频率 100Hz
    timer_receive = this->create_wall_timer(std::chrono::milliseconds(10), bind(&Serial::receive_callback, this));
    // 发送频率
    timer_send = this->create_wall_timer(std::chrono::milliseconds(1000), bind(&Serial::send_callback, this));
  }
};

int main(int argc, char **argv)
{
  std::string port;
  int baudrate;
  port = "/dev/ttyUSB0";
  baudrate = 921600;
  init(argc, argv);

  auto node = make_shared<Serial>("Serial_connect");

  node->declare_parameter<std::string>("port_name", port);
  node->declare_parameter<int>("buad", baudrate);

  node->get_parameter<std::string>("port_name", port);
  node->get_parameter<int>("buad", baudrate);

  ser.SetSerialPort(port, baudrate);
  while (ok())
  {
    if (!ser.Connect())
    {
      RCLCPP_INFO(node->get_logger(), "Serial Connectting!\n");
      sleep(1);
    }
    else
    {
      RCLCPP_INFO(node->get_logger(), "buadrate = %d!", baudrate);
      break;
    }
  }
  spin(node);
  ser.Disconnect();
  shutdown();
  return 0;
}

/**
 * @}
 */

/**
 * @}
 */

/**
 * @}
 */

/*********************************END OF FILE*********************************/
